#include "Stdafx.h"
#include <stdio.h>
#include <math.h>


/* Maximum number of data points allowed */
#define MAX_POINTS	500

/* An arbitrary tolerance factor */
#define EPSILON		1.0E-8

/* Commonly used macros */
#define ABS(a)          (((a) > 0) ? (a) : -(a))
#define SIGNBIT(a)      (((a) > 0) ? 0 : 1)
#define SQR(a)          ((a) * (a))
#define CUB(a)          ((a)*(a)*(a))
//#define MAX(a,b)        (((a) > (b)) ? (a) : (b))
//#define MIN(a,b)        (((a) < (b)) ? (a) : (b))

/* Some math libraries may not have the sincos() routine */
#ifdef _SINCOS_
void sincos();
#define SINCOS(x,s,c)   sincos(x,&s,&c)
#else
double _sin(), _cos();
#define SINCOS(x,s,c)   s=sin(x);c=cos(x)
#endif
#define SQRT(x) sqrt(fabs(x))


/* Parameters controlling MINPACK's lmdif() optimization routine. */
/* See the file lmdif.f for definitions of each parameter.        */
#define REL_SENSOR_TOLERANCE_ftol    1.0E-5      /* [pix] */
#define REL_PARAM_TOLERANCE_xtol     1.0E-7
#define ORTHO_TOLERANCE_gtol         0.0
#define MAXFEV                       (1000*n)
#define EPSFCN                       1.0E-16     /* Do not set to zero! */
#define MODE                         1           /* variables are scalled internally */
#define FACTOR                       100.0

struct camera_parameters {
	double    Ncx;		/* [sel]     Number of sensor elements in camera's x direction */
	double    Nfx;		/* [pix]     Number of pixels in frame grabber's x direction   */
	double    dx;		/* [mm/sel]  X dimension of camera's sensor element (in mm)    */
	double    dy;		/* [mm/sel]  Y dimension of camera's sensor element (in mm)    */
	double    dpx;		/* [mm/pix]  Effective X dimension of pixel in frame grabber   */
	double    dpy;		/* [mm/pix]  Effective Y dimension of pixel in frame grabber   */
	double    Cx;		/* [pix]     Z axis intercept of camera coordinate system      */
	double    Cy;		/* [pix]     Z axis intercept of camera coordinate system      */
	double    sx;		/* []        Scale factor to compensate for any error in dpx   */
};
struct calibration_constants {
	double    f;		/* [mm]          */
	double    kappa1;		/* [1/mm^2]      */
	double    p1;		/* [1/mm]        */
	double    p2;		/* [1/mm]        */
	double    Tx;		/* [mm]          */
	double    Ty;		/* [mm]          */
	double    Tz;		/* [mm]          */
	double    Rx;		/* [rad]	 */
	double    Ry;		/* [rad]	 */
	double    Rz;		/* [rad]	 */
	double    r1;		/* []            */
	double    r2;		/* []            */
	double    r3;		/* []            */
	double    r4;		/* []            */
	double    r5;		/* []            */
	double    r6;		/* []            */
	double    r7;		/* []            */
	double    r8;		/* []            */
	double    r9;		/* []            */
};

void load_cp_cc_data (FILE  *, struct camera_parameters *, struct calibration_constants *);
void      distorted_to_undistorted_sensor_coord(double Xd,double Yd,double& Xu,double& Yu,struct camera_parameters cp,struct calibration_constants cc);
void      image_coord_to_world_coord(double Xfd,double Yfd,double zw,double& xw,double& yw,struct camera_parameters cp,struct calibration_constants cc);